/* 
* @Author: obsidian
* @Date:   2015-12-16 10:18:52
* @Last Modified by:   obsidian
* @Last Modified time: 2016-04-25 20:45:56
*/
#include "pid.h"
#include "driver/sd/file.h"
#include "control/car.h"



static float get_pid(float error);
static void pid_read_sd(void);
static float get_double_loop_pid(float distance_error,float angle_error);
static float get_angle_pid(float error);
static void  pid_clear(void);
static void pid_init(void);
static void pid_save(void);

#define LINE_PID    {0.14,0.00,0.05}
#define LINE2_PID   {0.12,0.00,0.05}
#define LINE3_PID   {0.20,0.00,0.00}
#define LINE4_PID   {0.15,0.00,0.00}

struct Pid pid = 
{
    .pid_list = 
    {
        LINE2_PID,  // LINE_PID,
        LINE_PID,
        LINE2_PID,  // LINE2_PID,
        LINE_PID,  // LINE_PID,
        LINE2_PID,
        LINE_PID,  // LINE2_PID,
        LINE2_PID,  // LINE_PID,
        LINE_PID,
        LINE2_PID,  // LINE2_PID,
        LINE_PID,  // LINE_PID,
        LINE2_PID,
        {0.35,0,0.05},   // LINE2_PID, //75
        LINE3_PID,       // LINE_PID,
        LINE3_PID,
        LINE3_PID,       // {0.16,0.00,0.05},
        LINE3_PID,       // LINE_PID,
        LINE3_PID,
        LINE3_PID,       // {0.3,0.00,0.13},
        LINE3_PID,       // //{0.27,0.00,0.05},
        LINE4_PID,       // LINE_PID,
        LINE4_PID,
        LINE4_PID,       // {0.15,0.00,0.05},
        LINE4_PID,
        LINE4_PID,
        LINE4_PID,
        // {0.16,0.00,0.05},
        // LINE_PID,

        // {0.15,0.00,0.15},

        // LINE_PID,
        // {0.1,0.00,0.16},

        // LINE_PID,
        // {0.25,0.00,0.18},

        // LINE_PID,
        // {0.18,0.00,0.14},

        // LINE_PID,
    },
    .mend                   =   {0},    //mend[2]
    .error                  =   {0},    //error[2]
    .route_id               =   0,
    
    //function
    .init                   =   pid_init,
    .read_from_sd           =   pid_read_sd,
    .get_pid                =   get_pid,
    .get_double_loop_pid    =   get_double_loop_pid,
    .pid_clear              =   pid_clear,
    .save                   =   pid_save,
};


void pid_init()
{
    pid.pid_clear();
}
void pid_read_sd()
{
    unsigned int k = 0;
    car.strjoint("pid");
    f_open(&fil,car.path,FA_READ);
    
    if(fil.fs == 0){return;}
    f_read(&fil, (char *)pid.pid_list, sizeof(pid.pid_list), &k);
    file_close();
}

void pid_save()
{
    unsigned int k = 0;
    car.strjoint("pid");
    f_open(&fil,car.path,FA_WRITE | FA_CREATE_ALWAYS);
    if(fil.fs == 0){return;}
    f_write(&fil, (char *)pid.pid_list, sizeof(pid.pid_list), &k);
    file_close();
}



void pid_clear()
{
    unsigned char i = 0;
    for(i = 0;i < 24; i ++)
    {
        pid.mend[i] = 0;
        pid.error[i][0] = 0;
        pid.error[i][1] = 0;
    }
    //pid.route_id = 0;
}
float get_pid(float error)
{
    float A=0;
    float B=0;
    float C=0;
    float delta=0;
    
    A = pid.pid_list[pid.route_id].kd + pid.pid_list[pid.route_id].ki + pid.pid_list[pid.route_id].kp;
    B = - (pid.pid_list[pid.route_id].kp + 2 * pid.pid_list[pid.route_id].kd);
    C = pid.pid_list[pid.route_id].kd;
    
    delta = A*error + B*pid.error[pid.route_id][0] + C*pid.error[pid.route_id][1];
    pid.mend[pid.route_id] += delta;
    
    pid.error[pid.route_id][1] = pid.error[pid.route_id][0];
    pid.error[pid.route_id][0] = error;
    
    return pid.mend[pid.route_id];
}

float get_double_loop_pid(float distance_error,float angle_error)
{
    float A = 0;
    float B = 0;
    float C = 0;
    float angle_delta = 0;
    float distance_delta = 0;
    
    //distance_error = distance_error/pid.pid_list[DISTANCE].coff;
    //distance_error = distance_error * distance_error * distance_error / 10000;
    A = (pid.pid_list[ANGLE].kp*(1+1/pid.pid_list[ANGLE].ki+pid.pid_list[ANGLE].kd));
    B = -pid.pid_list[ANGLE].kp*(1+2*pid.pid_list[ANGLE].kd);
    C = pid.pid_list[ANGLE].kp*pid.pid_list[ANGLE].kd;
    
    angle_delta = A*angle_error + B*pid.error[ANGLE][0] + C*pid.error[ANGLE][1];
    pid.mend[ANGLE] += angle_delta;
    
    pid.error[ANGLE][1] = pid.error[ANGLE][0];
    pid.error[ANGLE][0] = angle_error;


    distance_error += angle_delta;
    A = (pid.pid_list[DISTANCE].kp*(1+1/pid.pid_list[DISTANCE].ki+pid.pid_list[DISTANCE].kd));
    B = -pid.pid_list[DISTANCE].kp*(1+2*pid.pid_list[DISTANCE].kd);
    C = pid.pid_list[DISTANCE].kp*pid.pid_list[DISTANCE].kd;
    
    distance_delta = A*distance_error + B*pid.error[DISTANCE][0] + C*pid.error[DISTANCE][1];
    
    pid.error[DISTANCE][1] = pid.error[DISTANCE][0];
    pid.error[DISTANCE][0] = distance_error;
    pid.mend[DISTANCE] += distance_delta;
    return pid.mend[DISTANCE];
}

float get_angle_pid(float error)
{
    float A=0;
    float B=0;
    float C=0;
    float delta=0;
    
    A = (pid.pid_list[ANGLE].kp*(1+1/pid.pid_list[ANGLE].ki+pid.pid_list[ANGLE].kd));
    B = -pid.pid_list[ANGLE].kp*(1+2*pid.pid_list[ANGLE].kd);
    C = pid.pid_list[ANGLE].kp*pid.pid_list[ANGLE].kd;
    
    delta = A*error + B*pid.error[ANGLE][0] + C*pid.error[ANGLE][1];
    pid.mend[ANGLE] += delta;
    
    pid.error[ANGLE][1] = pid.error[ANGLE][0];
    pid.error[ANGLE][0] = error;
    
    return pid.mend[ANGLE];
}
